#include "internal.h"
#include "Accelerometer3D.h"
#include "Accelerometer3D.tmh"
#include "WpdObjectProperties.h" 

const PROPERTYKEY g_SupportedThreeAxisProperties[] =
{
    SENSOR_PROPERTY_TYPE,
    SENSOR_PROPERTY_STATE,
    SENSOR_PROPERTY_CURRENT_REPORT_INTERVAL,
    SENSOR_PROPERTY_MIN_REPORT_INTERVAL,
    SENSOR_PROPERTY_PERSISTENT_UNIQUE_ID,
    SENSOR_PROPERTY_MANUFACTURER, 
    SENSOR_PROPERTY_MODEL,
    SENSOR_PROPERTY_SERIAL_NUMBER,
	SENSOR_PROPERTY_FRIENDLY_NAME,
    SENSOR_PROPERTY_DESCRIPTION, 
    SENSOR_PROPERTY_CONNECTION_TYPE,
    WPD_FUNCTIONAL_OBJECT_CATEGORY,
};

const PROPERTYKEY g_SupportedThreeAxisDataFields[] =
{
    SENSOR_DATA_TYPE_TIMESTAMP,
    SENSOR_DATA_TYPE_ACCELERATION_X_G,
    SENSOR_DATA_TYPE_ACCELERATION_Y_G,
    SENSOR_DATA_TYPE_ACCELERATION_Z_G,
};

Accelerometer3D::Accelerometer3D(WpdObjectProperties* pWpdObjectProperties): 
	SensorBase(pWpdObjectProperties) 
{
	m_strSensorName		= SENSOR_THREE_AXIS_FRIENDLY_NAME;
	m_strSensorId		= SENSOR_THREE_AXIS_ID;
	m_gdSensorUniqueId	= SENSOR_THREE_AXIS_GUID;
	m_gdSensorCategory	= SENSOR_CATEGORY_MOTION;
	m_gdSensorType		= SENSOR_TYPE_ACCELEROMETER_3D;
}

VOID Accelerometer3D::AddPropertyKeys(
	IPortableDeviceKeyCollection* pKeys)
{
    if (pKeys != NULL)
    {
        for (DWORD dwIndex = 0; dwIndex < ARRAYSIZE(g_SupportedThreeAxisProperties); dwIndex++)
        {
            pKeys->Add(g_SupportedThreeAxisProperties[dwIndex] );
        }
    }
}

VOID Accelerometer3D::AddDataFieldKeys(
	IPortableDeviceKeyCollection* pKeys)
{
    if (pKeys != NULL)
    {
        for (DWORD dwIndex = 0; dwIndex < ARRAYSIZE(g_SupportedThreeAxisDataFields); dwIndex++)
        {
            pKeys->Add(g_SupportedThreeAxisDataFields[dwIndex] );
        }
    }
}

// Strips the packet header value "934" from the
// packet of accelerometer data. (ie. 934001910070032 becomes 001910070032).
__int64 Remove3DAccelPacketHeader(__int64 llData)
{
    __int64 llFinalVal = 0;

    llFinalVal = llData % 1000000000000;
    return llFinalVal;
}

float RetrieveAccel3D(LONGLONG llVal, int pos)
{
	DoTraceMessage(MYDRIVER_ALL_INFO, L"%!FUNC! - pos = %d llVal = %lld",pos, llVal); 

	float fForce = 0.0;

	for (int i = 0; i < pos; i++)
		llVal /= 10000;

	fForce = (float)(llVal % 10000);

	DoTraceMessage(MYDRIVER_ALL_INFO,L"%!FUNC! - fForce = %f",fForce); 


	fForce -= 500.0;
	fForce /= 100.0; 

    return fForce;
}

HRESULT Accelerometer3D::OnGetDataValues(
    PROPERTYKEY &Key,
    IPortableDeviceValues*         pValues)
{
    HRESULT hr		= S_OK;

	float	fXAccel = 0.0;
    float	fYAccel = 0.0;
    float	fZAccel = 0.0;

    // Set G-force data   
    if (IsEqualPropertyKey(Key, SENSOR_DATA_TYPE_ACCELERATION_X_G))
    {
        //llData = Remove3DAccelPacketHeader(m_llSensorReading);
        fXAccel = RetrieveAccel3D((__int64)m_llSensorReading, 2);
        hr = pValues->SetFloatValue(SENSOR_DATA_TYPE_ACCELERATION_X_G, fXAccel);

		DoTraceMessage(MYDRIVER_ALL_INFO,L"%!FUNC! - fXAccel = %f",fXAccel); 
    } 
	else if (IsEqualPropertyKey(Key, SENSOR_DATA_TYPE_ACCELERATION_Y_G))
    {
        //llData = Remove3DAccelPacketHeader(m_llSensorReading);
        fYAccel = RetrieveAccel3D((__int64)m_llSensorReading, 1);
        hr = pValues->SetFloatValue(SENSOR_DATA_TYPE_ACCELERATION_Y_G, fYAccel);

		DoTraceMessage(MYDRIVER_ALL_INFO,L"%!FUNC! - fYAccel = %f",fYAccel); 
    }
	else if (IsEqualPropertyKey(Key, SENSOR_DATA_TYPE_ACCELERATION_Z_G))
    {
        //llData = Remove3DAccelPacketHeader(m_llSensorReading);
        fZAccel = RetrieveAccel3D((__int64)m_llSensorReading, 0);
        hr = pValues->SetFloatValue(SENSOR_DATA_TYPE_ACCELERATION_Z_G, fZAccel);

		DoTraceMessage(MYDRIVER_ALL_INFO,L"%!FUNC! - fZAccel = %f",fZAccel); 
	}
	return hr;
}